/*
 *
 *
 */

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

#include "fsl_debug_console.h"
#include "board.h"

#include "common.h"
#include "app.h"

uint8_t L_Speed = 0;
uint8_t R_Speed = 0;
uint8_t L_Near = 0;
uint8_t R_Near = 0;
uint8_t U_Turn = 0;
uint8_t Close_Flag = 0;

/*!
 * @brief Task responsible for printing of "Hello world." message.
 */
void hello_task(void *pvParameters)
{
    for (;;)
    {
        PRINTF("\nCleanBot  Hetzer_Chen @2018.\r\n");
        PRINTF("Start task creation.\r\n");

        // 创建LED指示灯任务
        if(xTaskCreate(StateLED_task, "StateLED_task", 128, NULL, StateLED_task_PRIORITY, NULL) != pdPASS)
        {
        	PRINTF("LED task creation failed!!!\r\n");
        	vTaskSuspend(NULL);
        }
        PRINTF("LED task has been created.\r\n");

        // 创建Shell任务
        if(xTaskCreate(Shell_task, "Shell_task", 256 , NULL, Shell_task_PRIORITY, NULL) != pdPASS)
        {
        	PRINTF("Shell task creation failed!!!\r\n");
        	vTaskSuspend(NULL);
        }
        PRINTF("Shell task has been created.\r\n");
        
        // 创建左电机任务
        if(xTaskCreate(L_MOTOR_task, "L Motor task", 128 , NULL, L_Motor_task_PRIORITY, NULL) != pdPASS)
        {
        	PRINTF("L MOTOR task creation failed!!!\r\n");
        	vTaskSuspend(NULL);
        }
        PRINTF("L MOTOR task has been created.\r\n");
       
        // 创建右电机任务
        if(xTaskCreate(R_MOTOR_task, "R Motor task", 128 , NULL, R_Motor_task_PRIORITY, NULL) != pdPASS)
        {
        	PRINTF("R MOTOR task creation failed!!!\r\n");
        	vTaskSuspend(NULL);
        }
        PRINTF("R MOTOR task has been created.\r\n");

        // VL53L0X测距任务
        if(xTaskCreate(VL53L0X_task, "VL53L0X task", 256 , NULL, VL53L0X_task_PRIORITY, NULL) != pdPASS)
        {
            PRINTF("VL53L0X task creation failed!!!\r\n");
            vTaskSuspend(NULL);
        }
        PRINTF("VL53L0X task has been created.\r\n");

        // 碰撞检测任务
        if(xTaskCreate(Collision_Detection_task, "Collision detection task", 256 , NULL, Collision_Detection_task_PRIORITY, NULL) != pdPASS)
        {
            PRINTF("Collision detection task creation failed!!!\r\n");
            vTaskSuspend(NULL);
        }
        PRINTF("Collision detection task has been created.\r\n");

        // 路径规划任务
        if(xTaskCreate(Route_Planning_task, "Route Planning task", 512 , NULL, Route_Planning_task_PRIORITY, NULL) != pdPASS)
        {
            PRINTF("Route Planning task creation failed!!!\r\n");
            vTaskSuspend(NULL);
        }
        PRINTF("Route Planning task has been created.\r\n");

        /* Enable GPIO pin interrupt */  
        GPIO_PortEnableInterrupts(GPIO4, 1U << 9U);
        GPIO_PortEnableInterrupts(GPIO4, 1U << 10U);

        vTaskSuspend(NULL);
    }
}
void StateLED_task(void *pvParameters)
{
    //uint16_t StateLED_Delay = 1000/portTICK_RATE_MS;
	for(;;)
	{
		USER_LED_TOGGLE();
        TaskDelay_ms(1000);
	}
}

void Shell_task(void *pvParameters)
{
	shell_context_struct user_context;
    
	/* Init SHELL */
    SHELL_Init(&user_context, SHELL_SendDataCallback, SHELL_ReceiveDataCallback, SHELL_Printf, "SHELL>> ");
    Command_Register();
    while(1)
    {
		//PRINTF("23");
        SHELL_Main(&user_context);
        
        TaskDelay_ms(100);
    }
    //vTaskSuspend(NULL);
}

void L_MOTOR_task(void *pvParameters)
{
    while(1)
    {
        
        
        TaskDelay_ms(1000);
    }
}

void R_MOTOR_task(void *pvParameters)
{
    while(1)
    {
        TaskDelay_ms(1000);
    }
}

void VL53L0X_task(void *pvParameters)
{
    uint8_t DeviceRangeStatusInternal;
    
    if(VL53L0X_Init() != 0)
        PRINTF("\r\n VL53L0X Init Error \r\n");

    while(1)
    {
        if(VL53L0X_Write_Byte() != 0)
            PRINTF("\r\n VL53L0X Write Byte Error \r\n");

        if(VL53L0X_Read_Byte() != 0)
            PRINTF("\r\n VL53L0X Read Byte Not ready \r\n");
        
        if(VL53L0X_Read_Buf() != 0)
            PRINTF("\r\n VL53L0X Read Buf Error \r\n");

        count[0] = makeuint16(gbuf[7], gbuf[6]);
        count[1] = makeuint16(gbuf[9], gbuf[8]);
        count[2] = makeuint16(gbuf[11], gbuf[10]);
        DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);

        Dis_ambient = count[0];
        Dis_signal = count[1];
        Dis_State = DeviceRangeStatusInternal;

        //PRINTF("\r\n ambient count = %5d signal count = %5d distance = %5d status = %d ",count[0],count[1],count[2],DeviceRangeStatusInternal);

        // 值有效
        if(DeviceRangeStatusInternal == 11)
        {
            DIST_RED_LED_ON();
            DIST_YEL1_LED_ON();
            DIST_YEL2_LED_ON();
            DIST_GRE1_LED_ON();
            DIST_GRE2_LED_ON();
            DIST_GRE3_LED_ON();

            Distance = count[2];

            if(Distance < 550)
            {
                DIST_RED_LED_OFF();
                if(Distance < 450)
                {
                    DIST_YEL1_LED_OFF();
                    if(Distance < 350)
                    {
                        DIST_YEL2_LED_OFF();
                        if(Distance < 250)
                        {
                            DIST_GRE1_LED_OFF();
                            if(Distance < 150)
                            {
                                DIST_GRE2_LED_OFF();
                            }
                        }
                    }
                }
            }
            
            Close_Flag = 0;
        }

        else if(DeviceRangeStatusInternal == 8)
        {
            Close_Flag ++;

            //if((count[0] >= 2000) && Close_Flag >= 5)
			if(count[0] >= 2000)
            {
				Close_Flag = 5;
                Distance = 0;
                DIST_RED_LED_OFF();
            }
        }
       
        TaskDelay_ms(200);
    }
}

void Route_Planning_task(void *pvParameters)
{
	//vTaskSuspend(NULL);
	
    while(1)
    {
        //ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        if((Distance < 170) && Crash_L == 0 && Crash_R == 0 && Start_Signal == 2)
        {
            L_Speed = 60;
            R_Speed = 65;
        }
        if((Distance >= 170) && Crash_L == 0 && Crash_R == 0 && Start_Signal == 2)
        {
            L_Speed = 82;
            R_Speed = 90;
        }

        L_MOTOR_CTRL(1, L_Speed);
        R_MOTOR_CTRL(1, R_Speed);

		if(Crash_L == 1 || Crash_R == 1)
        {
            L_MOTOR_CTRL(1, 0);
            R_MOTOR_CTRL(1, 0);
            TaskDelay_ms(200);

            L_MOTOR_CTRL(0, 70);
            R_MOTOR_CTRL(0, 70);
            TaskDelay_ms(500);

            //左侧撞击
            if(Crash_L == 1 && Crash_R != 1)
            {
                L_MOTOR_CTRL(1, 80);
                R_MOTOR_CTRL(0, 80);
                L_Near = 1;
            }
            //右侧撞击
            if(Crash_L != 1 && Crash_R == 1)
            {
                L_MOTOR_CTRL(0, 80);
                R_MOTOR_CTRL(1, 80);
                R_Near = 1;
            }
            //俩全撞
            if(Crash_L == 1 && Crash_R == 1)
            {
                L_MOTOR_CTRL(1, 80);
                R_MOTOR_CTRL(0, 80);
                U_Turn = 1;
            }
		
            TaskDelay_ms(1000);
            Crash_L = 0;
            Crash_R = 0;
        }

        TaskDelay_ms(10);
    }
}

void Collision_Detection_task(void *pvParameters)
{
    while(1)
    {
        // /* Update duty cycles for all 3 PWM signals */
        if(Start_Signal == 1)
        {
			TaskDelay_ms(700); 
			
            L_MOTOR_CTRL(1, 85);
            R_MOTOR_CTRL(1, 90);
            PWM_UpdatePwmDutycycle(PWM4, kPWM_Module_2, kPWM_PwmA, kPWM_SignedCenterAligned, 70);
            PWM_UpdatePwmDutycycle(PWM4, kPWM_Module_3, kPWM_PwmA, kPWM_SignedCenterAligned, 50);

            // /* Set the load okay bit for all submodules to load registers from their buffer */
            PWM_SetPwmLdok(PWM4, kPWM_Control_Module_0 | kPWM_Control_Module_1 | kPWM_Control_Module_2 | kPWM_Control_Module_3, true);
			
			TaskDelay_ms(300);
			Start_Signal = 2;

            //vTaskResume(Route_Planning_task);
        }
        if(U_Turn == 1)
        {
            TaskDelay_ms(2000);
            Crash_L = 1;
            U_Turn = 0;
        }
        // if(L_Near == 1)
        // {
        //     TaskDelay_ms(2000);

        //     L_MOTOR_CTRL(1, 80);
        //     R_MOTOR_CTRL(0, 80);

        //     Crash_L = 1;
        //     U_Turn = 0;
        // }
        // if(R_Near == 1)
        // {
        //     TaskDelay_ms(2000);

        //     L_MOTOR_CTRL(1, 80);
        //     R_MOTOR_CTRL(0, 80);

        //     Crash_L = 1;
        //     U_Turn = 0;
        // }

        TaskDelay_ms(10);
    }
}


